import os
from launch import LaunchDescription
from launch_ros.actions import LifecycleNode, Node
from launch_ros.descriptions import LifecycleNode as LifecycleNodeDesc
from launch.actions import DeclareLaunchArgument, RegisterEventHandler
from launch.event_handlers import OnProcessStart, OnExecutionComplete
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():

    # 获取包共享目录
    pkg_share = get_package_share_directory('radiation_source_state_machine')
    
    # 配置文件路径
    state_machine_params = os.path.join(pkg_share, 'config', 'state_machine.params.yaml')
    center_detection_params = os.path.join(pkg_share, 'config', 'center_detector.params.yaml')
    center_detection_second_params = os.path.join(pkg_share, 'config', 'center_detector_second.params.yaml')
    local_navigation_params = os.path.join(pkg_share, 'config', 'local_navigator.params.yaml')
    mqtt_cloud_sub_params = os.path.join(pkg_share, 'config', 'cloud_subscriber.yaml')
    fine_alignment_params = os.path.join(pkg_share, 'config', 'fine_aligner.params.yaml')
    gripper_control_params = os.path.join(pkg_share, 'config', 'gripper_communicator.params.yaml')
    
    
    # 状态机节点
    state_machine_node = Node(
        package='radiation_source_state_machine',
        executable='state_machine_main',
        name='radiation_source_state_machine',
        output='screen',
        parameters=[state_machine_params],
    )
    
    # 第一次点云中心检测节点
    center_detection_node = LifecycleNode(
        package='center_detection',
        executable='center_detector',
        name='center_detector',
        namespace='',
        output='screen',
        parameters=[center_detection_params],
        node_type='lifecycle_node',
    )
    

    # 导航节点
    local_navigation_node = LifecycleNode(
        package='local_navigation',
        executable='local_navigator',
        name='local_navigator',
        namespace='',
        output='screen',
        parameters=[local_navigation_params],
        node_type='lifecycle_node',
    )
    

    # mqtt订阅节点
    mqtt_cloud_sub_node = LifecycleNode(
        package='mqtt_cloud_sub',
        executable='cloud_subscriber',
        name='pointcloud_mqtt_subscriber',
        namespace='',
        output='screen',
        parameters=[mqtt_cloud_sub_params],
        node_type='lifecycle_node',
    )
    

    # 第二次点云中心检测节点
    center_detection_second_node = LifecycleNode(
        package='center_detection_second',
        executable='center_detector_second',
        name='center_detector_second',
        namespace='',
        output='screen',
        parameters=[center_detection_second_params],
        node_type='lifecycle_node',
    )


    # 精细调整节点
    fine_alignment_node = LifecycleNode(
        package='fine_alignment',
        executable='fine_aligner',
        name='fine_aligner',
        namespace='',
        output='screen',
        parameters=[fine_alignment_params],
        node_type='lifecycle_node',
    )
    
    # 抓手控制节点
    gripper_control_node = LifecycleNode(
        package='gripper_control',
        executable='gripper_communicator',
        name='gripper_communicator',
        namespace='',
        output='screen',
        parameters=[gripper_control_params],
        node_type='lifecycle_node',
    )
    
    return LaunchDescription([
        state_machine_node,
        center_detection_node,     
        local_navigation_node,
        mqtt_cloud_sub_node,
        center_detection_second_node, 
        fine_alignment_node,
        gripper_control_node
    ])